#include "_includes.h"

//////////////////////////////////////////////////////////////
const int time_all_sec = 105; // 总时间秒数
extern int speed_chassis;
/*
  振动提示相关函数
*/
timer t_usercontrol;
bool competition_on = false;
bool long_reminded_flag = false;
double dt_usercontrol = 0;

void ctrl_rumble_remind_reset() {
  t_usercontrol.reset();
  dt_usercontrol = 0;
  long_reminded_flag = false;
}

int get_usercontrol_remaining_time() {
  if (t_usercontrol.time() < time_all_sec * 1000)
    return time_all_sec * 1000 - t_usercontrol.time();
  else
    return 0;
}

void ctrl_rumble_remind_routine() {
  const int last_10_sec = time_all_sec - 10;
  const int big_remaining_time_sec = last_10_sec - 5; // last 15s
  /////////////////////////////////
  ctrl_clock_disp_reset();
  ctrl_rumble_remind_reset();

  while (!autonomous_running) {
    if (competition_on && Competition.isEnabled()) {
      if (!long_reminded_flag &&
          t_usercontrol.time(timeUnits::sec) > big_remaining_time_sec) {
        long_reminded_flag = true;
        // ctrl.rumble("---");
      }
      if (t_usercontrol.time(timeUnits::sec) >= last_10_sec + dt_usercontrol &&
          t_usercontrol.time(timeUnits::sec) <= last_10_sec + 10) {
        if (10 - dt_usercontrol > 5) { // 剩余时间10-5
          // ctrl.rumble(".");
          dt_usercontrol += 1;
        } else if (10 - dt_usercontrol > 3) { // 5-3
          dt_usercontrol += 0.5;
          // ctrl.rumble(".");
        } else if (10 - dt_usercontrol > 1) { // 3-1
          dt_usercontrol += 0.3;
          // ctrl.rumble(".");
        } else if (10 - dt_usercontrol > 0) { // 1-0
          dt_usercontrol += 0.5;
          // ctrl.rumble("-");
        }
      } // last 10sec
    }   // remind on
    wait(100);
  } // while
}

/*
  手动操控程序
*/
bool pitch_anti_slip = true; // 防倒
bool is_angle_up = false;
void _usercontrol(void) {
  thread_autonomous.interrupt(); // stop thread_autonomous
  autonomous_running = false;
  thread t(ctrl_rumble_remind_routine);
  wait(10); // 等待计时开启

  // double pitch_ground = get_pitch();

  thread t2(pne_init);
  thread t3(usecolor);
  thread t4(usetoarm);

  while (1) {

    // 除了左键的任意键都使机器人开始工作
    ActionUnit::set_all_units_motor_stop_status_on_working(
        ctrl.any_button_pressing() && !special_key.pressing());
    ////////////////////////////////////////
    if (!autonomous_running) { // 手动时段

      chassis.handle(ctrl.Axis3.position(), ctrl.Axis4.position(),
                     Hchassis::ctrlMode::m_voltage, 100, 15);// 最后一个数字是修改左右方向控制灵敏度
      special_key_handle();
      handle_suck();
      handle_chain();
      handle_hanger();
      handle_clamp();
      handle_arm();
      handle_sucklock();
      getstate();
      changecolor();

      ////////////////////////////////////////////////////////////////////

      //////////////////////////////////////////////////////////////////
      // 接上了场控
      if ((Competition.isFieldControl() || Competition.isCompetitionSwitch())) {
        competition_on = true;
        // if (get_usercontrol_remaining_time() < 600) {
        //   pne_occupying_unlock(); //接上场控后,最后0.5秒自动炸开
        // }
      } else {                                         // 未接场控
        if (competition_on) {                          // 计时训练手动
          if (get_usercontrol_remaining_time() == 0) { // 手动时间到
            ActionUnit::all_units_stop(coast);         // 机器停止10秒
            while_wait_timeout(!special_key.pressing(),
                               10000); // 停10s,左键退出
            competition_on = false;
          }
        } // competition_on
      }
    }         // 手动时段
    wait(10); // handle interval
  }           // while(1)
}
